#include <reactive_behavior/test/fake_localization_estimator.h>
#include <iostream>

using std::cout;
using std::endl;

using PlayerCc::rtod;

FakeLocalizationEstimator::FakeLocalizationEstimator(PlayerClient *player_client,
                                                     Position2dProxy *position2d_proxy,  
                                                     SyncedCurrentPose *synced_current_pose) :
                                                     player_client_(player_client),
                                                     position2d_proxy_(position2d_proxy),
                                                     synced_current_pose_(synced_current_pose) {}

void FakeLocalizationEstimator::operator()() {
  while (1) {
    player_client_->Read();
    player_pose2d pose;
    pose.px = position2d_proxy_->GetXPos();
    pose.py = position2d_proxy_->GetYPos();
    pose.pa = position2d_proxy_->GetYaw();
    synced_current_pose_->set_current_pose(pose);
    //cout << "POSE UPDATE: " << pose.px << " " << pose.py << " " << rtod(pose.pa) << endl;  // TEST
    boost::this_thread::sleep(boost::posix_time::milliseconds(1));  // Save CPU time.
  }
}
